package de.in.tum.msspp.Logger;

public class UprightOrientation {

    /**
     * Determine the orientation of the device in terms of the necessary
     * rotations around the z-axis (yaw), x-axis (pitch), and y-axis (roll),
     * in that specific order, with respect to the following coordinate
     * system:
     * 
     * ,------,         y           y-axis points at magnetic north
     * | DISP |        /            z-axis points at center of gravity
     * | DISP |   x---o             x-axis perpendicular to both y- and z-axis
     * | BTNS |       |
     * '------'       z
     *
     * 
     * @param accel
     *            an array of length 3 holding acceleration measurements
     * @param mag
     *            an array of length 3 holding geomagnetic field measurements
     * @param R
     *            an array of length 9 that will hold the resulting rotation
     *            matrix, which transforms coordinates given w.r.t. the above
     *            coordinate system to coordinates in the device's coordinate
     *            system, in the form
     *                  R[0]   R[1]   R[2]
     *                  R[3]   R[4]   R[5]
     *                  R[6]   R[7]   R[8]
     * @param orientation
     *            an array of length 3 that will hold the Euler angles for
     *            yaw, pitch, and roll given in radians
     * @return
     *            true iff the orientation could be computed
     */
    public static boolean getOrientation(float[] accel,
                                         float[] mag,
                                         float[] R,
                                         float[] orientation)
    {
        if (accel.length != 3 ||
            mag.length != 3 ||
            R.length != 9 ||
            orientation.length != 3)
        {
            throw new IllegalArgumentException();
        }

        // Upright orientation means a rotation about 90 degrees around the
        // X-axis according to the SensorEvent's coordinate system.
        // Thus the Y-axis becomes the Z-axis and the Z-axis becomes the
        // negative Y-axis.
        
        // Determine the (unscaled) z-axis.
        float zx = -accel[0];
        float zy = -accel[2];
        float zz = accel[1];
        
        // Determine a temporary (unscaled) y-axis.
        float tyx = mag[0];
        float tyy = mag[2];
        float tyz = -mag[1];

        // Determine the (unscaled) x-axis.
        float xx = tyy * zz - tyz * zy;
        float xy = tyz * zx - tyx * zz;
        float xz = tyx * zy - tyy * zx;
        
        // If the norm of the cross-product of the acceleration and geomagnetic
        // vectors is close to zero this means that the device is in or close
        // to free fall or at one of the magnetic poles where the magnetic field
        // is about perpendicular to the earth's surface.
        final float x_norm_squared = xx * xx + xy * xy + xz * xz; 
        if (x_norm_squared < 1e-1)
            return false;
        
        // Scale the x- and z-axes.
        final float x_norm = (float) Math.sqrt(x_norm_squared);
        xx /= x_norm;
        xy /= x_norm;
        xz /= x_norm;
        final float z_norm = (float) Math.sqrt(zx * zx + zy * zy + zz * zz);
        zx /= z_norm;
        zy /= z_norm;
        zz /= z_norm;
        
        // Recompute the y-axis so as to assure an orthogonal coordinate system.
        float yx = xy * zz - xz * zy;
        float yy = xz * zx - xx * zz;
        float yz = xx * zy - xy * zx;
        
        // Now the rotation matrix R is as follows:
        R[0] = xx;  R[1] = yx;  R[2] = zx;
        R[3] = xy;  R[4] = yy;  R[5] = zy;
        R[6] = xz;  R[7] = yz;  R[8] = zz;
        assert(
            // R is a valid rotation matrix <=> det(R) = +1
            Math.abs(xx * yy * zz + yx * zy * xz + zx * xy * yz -
                     zx * yy * xz - yx * xy * zz - xx * zy * yz - 1.0)
            < 1e-2
        );
        
        // With respect to a rotation order of yaw, pitch, roll this ultimately
        // leads to:
        orientation[0] = (float) Math.atan2(-R[3], R[4]);   // yaw
        orientation[1] = (float) Math.asin(R[5]);           // pitch
        orientation[2] = (float) Math.atan2(-R[2], R[8]);   // roll
        
        return true;
    }
    
}